/*
  gps.c
  The GPS functions for the MAKE Controller

  Ported by:  Andrew Stiegmann
  
 */

#include <stdio.h>
#include <serial.h>
#include <ctype.h>
#include <string.h>
#include <rtos.h>
#include <appled.h>
#include <error.h>

#include "gps.h"

// Useful Definitions
#define POLLING_WAIT_PERIOD 50

#define BAUD                4800    // Baud rate = 4800 
#define PARITY              0       // No parity
#define STOP_BITS           1       // One stop bit
#define DATA_BITS           8       // 8 bits for a char

#define CR                  0x0D    // Some ASCII character defs.  Probably not necessary, but what the hell.
#define LF                  0x0A
#define ESC                 0x1B
#define BEEP                0x07

#define SPACE	            0x20
#define COMMA	            0x2C
#define MAXSIZE             85	    // GPS at most, sends 82 or so chars per message string.  So set maximum to 85 

// Local Prototypes 
int   GPS_SetSerialPortNMEA( void );
uchar GPS_Serial_GetChar( void );
uint  NMEA_CommaParse( uint lcp, uchar data[], char result[] );
void  NMEA_ParseGPGGA( uchar stringRead[], GPS_POSData *pData );
//void  NMEA_ParseGPVTG( uchar stringRead[], GPS_NAVData *pData );
void  NMEA_ParseGPRMB( uchar stringRead[], GPS_NAVData *pData );
int   NMEA_ReadString( uchar stringRead[] );

void GPS_TestProg( void *p)
{
  //This simple program turns LED 1 on when it establishes a GPS fix.
  (void)p;
  GPS_POSData posData;
  
  //Init the Serial Port
  GPS_SetSerialPortNMEA();
  if(Serial_GetBaud() != BAUD)
		AppLed_SetState(1,1);
		
  while(1)
    {
      //continue;
      GPS_GetPOSData(&posData);
      if(posData.fix)
	AppLed_SetState(0,1);
      else
	AppLed_SetState(0,0);
    }		
  
}

int GPS_SetSerialPortNMEA( void ) 
{
  //Sets the appropriate settings of the serial port for NMEA

  int rMsg;
  
  rMsg = Serial_SetActive(1);
  if( rMsg != CONTROLLER_OK )
    return rMsg;

  rMsg = Serial_SetBaud(BAUD);
  if( rMsg != CONTROLLER_OK )
    return rMsg;

  rMsg = Serial_SetParity(PARITY);
  if( rMsg != CONTROLLER_OK )
    return rMsg;

  rMsg = Serial_SetStopBits(STOP_BITS);
  if( rMsg != CONTROLLER_OK )
    return rMsg;

  rMsg = Serial_SetBits(DATA_BITS);
  if( rMsg != CONTROLLER_OK )
    return rMsg;

  /*	
	AppLed_SetState(0,1);
	Sleep(500);
	AppLed_SetState(0,0);
  */  
  return CONTROLLER_OK;
}

uchar GPS_Serial_GetChar( void )
{
  //a blocking version of Serial_GetChar()

  uchar ucRet; 
  
  // Because we do not have an interrupt we can watch (at least I have not found one),
  // we must poll the serial interface until we find one.  Use the sleep method to make sure
  // other processes get equal time on the cpu.
  while( !Serial_GetReadable() )
    Sleep( POLLING_WAIT_PERIOD );
	
  /*
  AppLed_SetState(3,1);
  Sleep(2);
  AppLed_SetState(3,0);
  */

  Serial_Read(&ucRet,1,0);
  
  return ucRet;
}

unsigned int NMEA_CommaParse(unsigned int lcp, uchar data[], char result[])
{
   //Parses out the words between commas, returns last comma position
  //lcp = Last Comma Position
  //Data = string data

  int j, k;

  if( data[lcp] != CR)
    lcp++;
  else
    {
      // This means we started at the end of the string.
      result[0] = '\0';
      return lcp;
    }

  j = lcp;
  k = 0;
  while(data[j] != COMMA && data[j] != '*' ) 
    {
      result[k] = data[j];
      j++;
      k++;
    }	      
  result[k] = '\0';

  return j;
}

void NMEA_ParseGPGGA( uchar stringRead[], GPS_POSData *pData)
{
  uint lastCommaPosition = 0;
  char tempString[12];
  /*
    Parse a GPGGA string.  It has 14 commas total.  Its NMEA sentence structure is:
    
    $GPGAA,hhmmss.ss,ddmm.mmmm,n,dddmm.mmmm,e,q,ss,y.y,a.a,z,g.g,z,t.t,iii*CC
    |    |    |    |    |    |    |    |    |    |    |    |    |    |    |
    0   	   1         2         3         4         5         6         7
    0123456789012345678901234567890123456789012345678901234567890123456789012
    
    where:
    
    GPGAA		: GPS fixed data identifier
    hhmmss.ss	: Coordinated Universal Time (UTC), also known as GMT
    ddmm.mmmm,n	: Latitude in degrees, minutes and cardinal sign
    dddmm.mmmm,e	: Longitude in degrees, minutes and cardinal sign
    q		: Quality of fix.  1 = there is a fix
    ss		: Number of satellites being used
    y.y		: Horizontal dilution of precision
    a.a,M		: GPS antenna altitude in meters
    g.g,M		: geoidal separation in meters
    t.t		: Age of the defferential correction data
    iiii		: Deferential station's ID
    *CC		: checksum for the sentence		
    */
  
  
  //Modified here to Ignore Time.  We Do Not need it.
  // Get UTC time 
  
  // start of time field 
  lastCommaPosition = 6;
  lastCommaPosition = NMEA_CommaParse(lastCommaPosition,stringRead,tempString);
  
  //Now handle the data in tempString, which is the time.
  sscanf(tempString, "%f", &(pData->UTCTime));
  
  // Get lattitude: ddmm.mmmm 
  lastCommaPosition = NMEA_CommaParse(lastCommaPosition,stringRead,tempString);
  
  //Parse Out the Latitude
  sscanf(tempString, "%f", &(pData->latitude));
  pData->latDeg = (int)(pData->latitude/100);
  pData->latMin = (float)(pData->latitude - pData->latDeg*100);

  // Get lattitude Cardinal direction 
  lastCommaPosition = NMEA_CommaParse(lastCommaPosition,stringRead,tempString);
		  
  //Parse out the Cardinal character
  pData->latSign = tempString[0];
  
  // Get longitude: dddmm.mmmm 
  lastCommaPosition = NMEA_CommaParse(lastCommaPosition,stringRead,tempString);
	      
  //Parse out the Longitude
  sscanf(tempString, "%f", &(pData->longitude));
  pData->longDeg = (int)(pData->longitude/100);
  pData->longMin = (float)(pData->longitude - pData->longDeg*100);
  
  // Get longitude Cardinal direction 
  lastCommaPosition = NMEA_CommaParse(lastCommaPosition,stringRead,tempString);
  
  //Parse the cardinal direction
  pData->longSign = tempString[0];
  
  //Get Fix Data
  lastCommaPosition = NMEA_CommaParse(lastCommaPosition,stringRead,tempString);
  
  //Parse out fix
  sscanf(tempString, "%d", &(pData->fix));
  
  //Get # Sat data
  lastCommaPosition = NMEA_CommaParse(lastCommaPosition,stringRead,tempString);
  
  //Parse out number of Satellites
  sscanf(tempString, "%d", &(pData->numSats));

  //Dont have a need for the rest.  Ingnoring it.
  
  return;
}

/*
void NMEA_ParseGPVTG(char* stringRead, GPS_POSData *pData)
{
  uint lcp = 0;
  uchar tempString[12];
  uint i;    // Dummy counter 
  
    Parse a GPVTG string.  It has 8 commas total.  Its NMEA sentence structure is:
    
    $GPVTG,t,T,,,s.ss,N,s.ss,K*hh
    |    |    |    |    |    |    |    |    |    |    |    |    |    |    |
    0   	   1         2         3         4         5         6         7
    0123456789012345678901234567890123456789012345678901234567890123456789012
    
    where:
    
    GPVTG  : GPS Track Made Good and Ground Speed
    1	   : Track made good
    2      : Fixed text 'T' indicates that track made good is relative to true north
    3      : not used
    4      : not used
    5      : Speed over ground in knots
    6      : Fixed text 'N' indicates that speed over ground in in knots (IGNORE)
    7      : Speed over ground in kilometers/hour
    8      : Fixed text 'K' indicates that speed over ground is in kilometers/hour (IGNORE)
    9      : Checksum
    
  
  
  
    // I only care about speed in KM/H at the time of writing this.
    //So lets skip ahead with a nice little for loop
   
  lcp = 6;     //The start of the data in the string
  for(i=0; i < 7; lcp++)
    {
      if( stringRead[lcp] == COMMA )
	i++;
    }
  //Parse Out the speed
  lcp = NMEA_CommaParse(lcp,stringRead,tempString);
  sscanf((char*)tempString, "%f", &(pData->speed));
  
  //Dont have a need for the rest yet.  Ingnore it.
  
  return;
}
*/

void NMEA_ParseGPRMB( uchar stringRead[], GPS_NAVData* pData )
{
  uint lcp, i;
  char tempString[12];  
  /*
      Parse a GPVTG string.  It has 8 commas total.  Its NMEA sentence structure is:
    
    $GPRMB,A,0.66,L,003,004,4917.24,N,12309.57,W,001.3,052.5,000.5,V*20
    |    |    |    |    |    |    |    |    |    |    |    |    |    |    |
    0   	   1         2         3         4         5         6         7
    0123456789012345678901234567890123456789012345678901234567890123456789012
    
    where:
    
    A            Data status A = OK, V = Void (warning)
    0.66         Cross-track error (nautical miles, 9.99 max),
    L            Steer Left to correct (or R = right)
    003          Origin waypoint ID
    004          Destination waypoint ID
    4917.24      Destination waypoint latitude 49 deg. 17.24 min. 
    N            North or South
    12309.57     Destination waypoint longitude 123 deg. 09.57 min. 
    W            East or West
    001.3        Range to destination, nautical miles (999.9 max)
    052.5        True bearing to destination
    000.5        Velocity towards destination, knots
    V            Arrival alarm  A = arrived, V = not arrived
    *20          checksum  
  */
  
  // First handle the data in tempString, which is the if data is valid.
  lcp = 6;
  lcp = NMEA_CommaParse( lcp, stringRead, tempString );
  if( tempString[0] == 'A' )
    pData->isValid = true;
  else
    pData->isValid = false;
  
  // Handle cross track error
  lcp = NMEA_CommaParse( lcp, stringRead, tempString );
  sscanf(tempString, "%f", &(pData->crossTrackErr));
  
  // Handle Steering Direction
  lcp = NMEA_CommaParse( lcp, stringRead, tempString );
  pData->steeringDir = tempString[0];

  // Skip the next 6, cause I don't think they are too useful at the moment
  for( i=0; i<6; i++)
    lcp = NMEA_CommaParse( lcp, stringRead, tempString );

  // Range to dest
  lcp = NMEA_CommaParse( lcp, stringRead, tempString );
  sscanf(tempString, "%f", &(pData->rangeToDest));

  // Bearing to dest
  lcp = NMEA_CommaParse( lcp, stringRead, tempString );
  sscanf(tempString, "%f", &(pData->bearingToDest));

  // Velocity towards dest
  lcp = NMEA_CommaParse( lcp, stringRead, tempString );
  sscanf(tempString, "%f", &(pData->velocityTowardsDest));

  // at Destination?
  lcp = NMEA_CommaParse( lcp, stringRead, tempString );
  if( tempString[0] == 'A' )
    pData->atDest = true;
  else
    pData->atDest = false;

  return;
}

int NMEA_ReadString( uchar stringRead[] )
{
  /*
    Reads an NMEA string in from the input device to the str array.  Note that
    str must be at least 80 characters long, unless you want overflow.
  */
  int i;
  uchar charRead;

  while( Serial_GetActive() )
    {
      charRead = GPS_Serial_GetChar();  // Read a char from the Serial Buf
      if( charRead == '$' )             // GPS messages start with $ char 
	{           
	  /*
	  //debug LED
	  AppLed_SetState(2,1);
	  Sleep(10);
	  AppLed_SetState(2,0);
	  */
	  
	  i = 0;
	  stringRead[i] = charRead;
	  do 
	    {
	      charRead = GPS_Serial_GetChar();
	      if( (charRead != '\0') && (isalnum(charRead) ||  isspace(charRead) || ispunct(charRead)) ) 
		{
		  i++;
		  stringRead[i] = charRead;
		}
	    } while(charRead != CR);
	  
	  // By this point, a complete GPS string has been read
	  // Append the null terminator to the string read 
	  stringRead[i+1] = '\0';
	  return CONTROLLER_OK;
	}
    }
  
  //If here, then the Device from which we are reading data has gone inactive
  return CONTROLLER_ERROR_NOT_OPEN;
}

int GPS_GetPOSData( GPS_POSData *pData)
{
  uchar	 stringRead[MAXSIZE]; 	// Buffer collects chars read from GPS 
  int    rVal;                  // Used for remembering return values.
  uint	 msgsRead = 0;	
  
  while( msgsRead < 2 ) 
    {
      // Read in an NMEA string
      rVal = NMEA_ReadString( stringRead );
      if( rVal != CONTROLLER_OK )
	// Then there was a problem.  Return rVal.
	return rVal;
	
      if(stringRead[3] == 'G' && stringRead[4] == 'G' && stringRead[5] == 'A') 
	{
	  // Found GPGGA sentence
	  NMEA_ParseGPGGA( stringRead, pData);
	  return CONTROLLER_OK;
	  
	} 
      /*
      else if(stringRead[3] == 'V' && stringRead[4] == 'T' && stringRead[5] == 'G')
	{
	  // Found GPVTG sentence
	  makeParseGPVTG(stringRead,pData);
	  msgsRead++;
	}
      
      //Now that we have read a sentense, check if we have read enough to return all pData
      if(msgsRead >= 2)
	return 0;
      */

    }
  return CONTROLLER_OK;
}

int GPS_GetNAVData( GPS_NAVData *pData )
{
  uchar	 stringRead[MAXSIZE]; 	// Buffer collects chars read from GPS 
  int    rVal;                  // Used for remembering return values.
  
  while( true ) 
    {
      // Read in an NMEA string
      rVal = NMEA_ReadString( stringRead );
      if( rVal != CONTROLLER_OK )
	// Then there was a problem.  Return rVal.
	return rVal;
	
      // See if the sentense we read is the one we want
      if(stringRead[3] == 'R' && stringRead[4] == 'M' && stringRead[5] == 'B') 
	{
	  //Found GPRMB sentence
	  NMEA_ParseGPRMB(stringRead,pData);
	  return CONTROLLER_OK;
	}
    }
}
